/*
Automatic clinic tests using a robot arm.
Copyright (C) 2010  CIC-CPC, CHU Gabriel Montpied
All other trademarks, logos and copyrights are the property
of their respective owners.

This file is part of cpc-painbot.

cpc-painbot is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

cpc-painbot is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with cpc-painbot.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "zone.h"

#include <QDebug>

#include "positionbras.h"

Zone::Zone(QObject *parent) :
        QObject(parent)
{
}

bool Zone::aEteUtilisee(const PositionBras &pos) const
{

    return ontEteUtilisees(pos.x(), pos.y());
}

bool Zone::ontEteUtilisees(qreal x, qreal y) const
{
    QString serial = QString("%1,%2").arg(x).arg(y);

    return utilisees_.contains(serial);
}

void Zone::positionLibre(PositionBras &pos)
{
    qreal x, y;

    chercherCoordonnees(x, y);
    pos.setX(x);
    pos.setY(y);
}

bool Zone::contient(qreal x, qreal y) const
{
    qreal dx = x - centre().x();
    qreal dy = y - centre().y();
    qreal distance2 = dx*dx + dy*dy;

    qDebug() << "Zone::contient :"
            << distance2 << "<=" << rayonMax2()
            << "et" << distance2 << ">=" << rayonMin2();

    return distance2 <= rayonMax2()
            && distance2 >= rayonMin2();
}

bool Zone::contient(const PositionBras &pos) const
{
    return contient(pos.x(), pos.y());
}

void Zone::chercherCoordonnees(qreal &x, qreal &y)
{
    do {
        // Recherche d'un point dans la couronne.
        // rho dans [rmin,rmax]
        qreal rho = rayonMin() + (qrand() * (rayonMax() - rayonMin()))/RAND_MAX;
        // theta dans [0,2pi]
        qreal theta = (qrand() * M_2_PI)/RAND_MAX;

        // Application des coordonnées.
        x = centre().x() + rho * qBound(qreal(-1), cos(theta), qreal(1));
        y = centre().y() + rho * qBound(qreal(-1), sin(theta), qreal(1));

        qDebug() << "Centre de la Zone :" << centre();
        qDebug().nospace() << "(rho = " << rho << ", theta = " << theta << ')';
        qDebug().nospace() << "(" << x << ',' << y << ')';
        Q_ASSERT(contient(x, y));
    } while ( ontEteUtilisees(x, y) );

    qDebug().nospace() << "Zone::positionLibre(" << x << ',' << y << ')';
}
